#include "LLRL.h"
/*
Robots parameters
*/


MAT	*PUMA560_DH(MAT *p560_DH)
{
	p560_DH = m_resize(p560_DH,6,5);

	p560_DH->me[0][0]=M_PI/2.0;  p560_DH->me[0][1]=0.0;    p560_DH->me[0][2]=0.0;  p560_DH->me[0][3]=0.0;     p560_DH->me[0][4]=0.0;  
	p560_DH->me[1][0]=0.0;     p560_DH->me[1][1]=0.4318; p560_DH->me[1][2]=0.0;  p560_DH->me[1][3]=0.0;     p560_DH->me[1][4]=0.0;  
	p560_DH->me[2][0]=-M_PI/2.0; p560_DH->me[2][1]=0.0203; p560_DH->me[2][2]=0.0;  p560_DH->me[2][3]=0.15005; p560_DH->me[2][4]=0.0;  
	p560_DH->me[3][0]=M_PI/2;	   p560_DH->me[3][1]=0.0;	 p560_DH->me[3][2]=0.0;	 p560_DH->me[3][3]=0.4318;	p560_DH->me[3][4]=0.0;	 
	p560_DH->me[4][0]=-M_PI/2;   p560_DH->me[4][1]=0.0;	 p560_DH->me[4][2]=0.0;	 p560_DH->me[4][3]=0.0;	    p560_DH->me[4][4]=0.0;	 
	p560_DH->me[5][0]=0.0;	   p560_DH->me[5][1]=0.0;	 p560_DH->me[5][2]=0.0;	 p560_DH->me[5][3]=0.0;	    p560_DH->me[5][4]=0.0;	 

	return p560_DH;
}

MAT	*PUMA560(MAT *p560)
{
	p560= m_resize(p560,6,15);

	p560->me[0][0]=0.0;   p560->me[0][1]=0.0;     p560->me[0][2]=0.0;     p560->me[0][3]=0.0;    p560->me[0][4]=0.0;     p560->me[0][5]=0.35;     p560->me[0][6]=0.0;     p560->me[0][7]=0.0;   p560->me[0][8]=0.0;   p560->me[0][9]=0.0;  p560->me[0][10]=0.0002;    p560->me[0][11]= -62.6111;  p560->me[0][12]= 0.00148;   p560->me[0][13]= 0.395;    p560->me[0][14]= -0.435;
	p560->me[1][0]=17.4;  p560->me[1][1]=-0.3638; p560->me[1][2]=0.006;   p560->me[1][3]=0.2275; p560->me[1][4]=0.13;    p560->me[1][5]=0.524;    p560->me[1][6]=0.539;   p560->me[1][7]=0.0;   p560->me[1][8]=0.0;   p560->me[1][9]=0.0;  p560->me[1][10]=0.0002;    p560->me[1][11]=107.815;    p560->me[1][12]= 0.000817;  p560->me[1][13]= 0.126;    p560->me[1][14]= -0.071; 
	p560->me[2][0]=4.8;   p560->me[2][1]=-0.0203; p560->me[2][2]=-0.0141; p560->me[2][3]=0.070;  p560->me[2][4]=0.066;   p560->me[2][5]=0.086;    p560->me[2][6]=0.0125;  p560->me[2][7]=0.0;   p560->me[2][8]=0.0;   p560->me[2][9]=0.0;  p560->me[2][10]=0.0002;    p560->me[2][11]=-53.7063;   p560->me[2][12]=0.00138;    p560->me[2][13]=0.132;     p560->me[2][14]=-0.105;
	p560->me[3][0]=0.82;  p560->me[3][1]=0.0;	  p560->me[3][2]=0.019;   p560->me[3][3]=0.0;	 p560->me[3][4]=0.0018;  p560->me[3][5]=0.0013;   p560->me[3][6]=0.0018;  p560->me[3][7]=0.0;   p560->me[3][8]=0.0;   p560->me[3][9]=0.0;  p560->me[3][10]=0.000033;  p560->me[3][11]=76.0364;	  p560->me[3][12]=0.0000712;  p560->me[3][13]=0.0112;	 p560->me[3][14]=-0.0169;
	p560->me[4][0]=0.34;  p560->me[4][1]=0.0;	  p560->me[4][2]=0.0;	  p560->me[4][3]=0.0;	 p560->me[4][4]=0.0003;  p560->me[4][5]=0.0004;   p560->me[4][6]=0.0003;  p560->me[4][7]=0.0;	p560->me[4][8]=0.0;   p560->me[4][9]=0.0;  p560->me[4][10]=0.000033;  p560->me[4][11]=71.923;	  p560->me[4][12]=0.0000826;  p560->me[4][13]=0.00926;   p560->me[4][14]=-0.0145;
	p560->me[5][0]=0.09;  p560->me[5][1]=0.0;	  p560->me[5][2]=0.0;	  p560->me[5][3]=0.032;  p560->me[5][4]=0.00015; p560->me[5][5]=0.00015;  p560->me[5][6]=0.00004; p560->me[5][7]=0.0;	p560->me[5][8]=0.0;   p560->me[5][9]=0.0;  p560->me[5][10]=0.000033;  p560->me[5][11]=76.686;	  p560->me[5][12]=0.0000367;  p560->me[5][13]=0.00396;   p560->me[5][14]=-0.0105;

	return p560;
}

MAT	*PA10(MAT *pa10)
{
	pa10= m_resize(pa10,7,15);

	pa10->me[0][0]=9.78;   pa10->me[0][1]=0.0;     pa10->me[0][2]=0.0;       pa10->me[0][3]=-0.166;    pa10->me[0][4]=0.110697;     pa10->me[0][5]=0.084268;     pa10->me[0][6]=0.054080;     pa10->me[0][7]=0.000005;   pa10->me[0][8]=0.000518;   pa10->me[0][9]=0.000345;  pa10->me[0][10]=0;    pa10->me[0][11]= 0;  pa10->me[0][12]= 0;   pa10->me[0][13]= 0;    pa10->me[0][14]= 0; 
	pa10->me[1][0]=8.41;   pa10->me[1][1]=0.0;	   pa10->me[1][2]=-0.0632;   pa10->me[1][3]=0.0;	   pa10->me[1][4]=0.177079;     pa10->me[1][5]=0.018440;     pa10->me[1][6]=0.173903;     pa10->me[1][7]=0.000257;   pa10->me[1][8]=0.0;        pa10->me[1][9]=0.0;		  pa10->me[1][10]=0;    pa10->me[1][11]=0;    pa10->me[1][12]= 0;  pa10->me[1][13]= 0;    pa10->me[1][14]= 0;   
	pa10->me[2][0]=3.51;   pa10->me[2][1]=0.0;     pa10->me[2][2]=0.0;		 pa10->me[2][3]=-0.112;	   pa10->me[2][4]=0.032979;     pa10->me[2][5]=0.017031;     pa10->me[2][6]=0.022417;     pa10->me[2][7]=0.0;        pa10->me[2][8]=0.000027;   pa10->me[2][9]=0.000002;  pa10->me[2][10]=0;    pa10->me[2][11]=0;   pa10->me[2][12]=0;    pa10->me[2][13]=0;     pa10->me[2][14]=0;
	pa10->me[3][0]=4.31;   pa10->me[3][1]=0.0;	   pa10->me[3][2]=-0.046;    pa10->me[3][3]=0.0;	   pa10->me[3][4]=0.051762;     pa10->me[3][5]=0.006329;     pa10->me[3][6]=0.051407;     pa10->me[3][7]=-0.000577;  pa10->me[3][8]=0.0;        pa10->me[3][9]=0.0;		  pa10->me[3][10]=0;  pa10->me[3][11]=0;  pa10->me[3][12]=0;  pa10->me[3][13]=0;	 pa10->me[3][14]=-0;  
    pa10->me[4][0]=3.45;   pa10->me[4][1]=0.0;	   pa10->me[4][2]=-0.0632;	 pa10->me[4][3]=0.0;	   pa10->me[4][4]=0.076687;     pa10->me[4][5]=0.077392;     pa10->me[4][6]=0.003233;     pa10->me[4][7]=0.0;	     pa10->me[4][8]=0.0;        pa10->me[4][9]=0.000960;  pa10->me[4][10]=0;  pa10->me[4][11]=0;  pa10->me[4][12]=0;  pa10->me[4][13]=0;   pa10->me[4][14]=0;
	pa10->me[5][0]=1.46;   pa10->me[5][1]=0.0;	   pa10->me[5][2]=0.003;	 pa10->me[5][3]=0.0;       pa10->me[5][4]=0.012500;     pa10->me[5][5]=0.001431;     pa10->me[5][6]=0.012500;     pa10->me[5][7]=0.0;	     pa10->me[5][8]=0.0;        pa10->me[5][9]=0.0;		  pa10->me[5][10]=0;  pa10->me[5][11]=0;	  pa10->me[5][12]=0;  pa10->me[5][13]=0;   pa10->me[5][14]=0;
	pa10->me[6][0]=0.24;   pa10->me[5][1]=0.0;	   pa10->me[5][2]=0.0;		 pa10->me[5][3]=0.0;       pa10->me[5][4]=0.001575;     pa10->me[5][5]=0.001575;     pa10->me[5][6]=0.000131;     pa10->me[5][7]=0.0;	     pa10->me[5][8]=0.0;        pa10->me[5][9]=0.0;		  pa10->me[5][10]=0;  pa10->me[5][11]=0;	  pa10->me[5][12]=0;  pa10->me[5][13]=0;   pa10->me[5][14]=0;

	return pa10;
}

/*MAT	*PA10(MAT *pa10)
{
  pa10= m_resize(pa10,7,15);

  pa10->me[0][0]=9.78;   pa10->me[0][1]=0.0;     pa10->me[0][2]=0.0;       pa10->me[0][3]=-0.166;    pa10->me[0][4]=0.110697;     pa10->me[0][5]=0.084268;     pa10->me[0][6]=0.054080;     pa10->me[0][7]=0.000005;   pa10->me[0][8]=0.000518;   pa10->me[0][9]=0.000345;  pa10->me[0][10]=0.0002;    pa10->me[0][11]= -62.6111;  pa10->me[0][12]= 0.00148;   pa10->me[0][13]= 0.395;    pa10->me[0][14]= -0.435;
  pa10->me[1][0]=8.41;   pa10->me[1][1]=0.0;	   pa10->me[1][2]=-0.0632;   pa10->me[1][3]=0.0;	   pa10->me[1][4]=0.177079;     pa10->me[1][5]=0.018440;     pa10->me[1][6]=0.173903;     pa10->me[1][7]=0.000257;   pa10->me[1][8]=0.0;        pa10->me[1][9]=0.0;		  pa10->me[1][10]=0.0002;    pa10->me[1][11]=107.815;    pa10->me[1][12]= 0.000817;  pa10->me[1][13]= 0.126;    pa10->me[1][14]= -0.071; 
  pa10->me[2][0]=3.51;   pa10->me[2][1]=0.0;     pa10->me[2][2]=0.0;		 pa10->me[2][3]=-0.112;	   pa10->me[2][4]=0.032979;     pa10->me[2][5]=0.017031;     pa10->me[2][6]=0.022417;     pa10->me[2][7]=0.0;        pa10->me[2][8]=0.000027;   pa10->me[2][9]=0.000002;  pa10->me[2][10]=0.0002;    pa10->me[2][11]=-53.7063;   pa10->me[2][12]=0.00138;    pa10->me[2][13]=0.132;     pa10->me[2][14]=-0.105;
  pa10->me[3][0]=4.31;   pa10->me[3][1]=0.0;	   pa10->me[3][2]=-0.046;    pa10->me[3][3]=0.0;	   pa10->me[3][4]=0.051762;     pa10->me[3][5]=0.006329;     pa10->me[3][6]=0.051407;     pa10->me[3][7]=-0.000577;  pa10->me[3][8]=0.0;        pa10->me[3][9]=0.0;		  pa10->me[3][10]=0.000033;  pa10->me[3][11]=76.0364;	  pa10->me[3][12]=0.0000712;  pa10->me[3][13]=0.0112;	 pa10->me[3][14]=-0.0169;
  pa10->me[4][0]=3.45;   pa10->me[4][1]=0.0;	   pa10->me[4][2]=-0.0632;	 pa10->me[4][3]=0.0;	   pa10->me[4][4]=0.076687;     pa10->me[4][5]=0.077392;     pa10->me[4][6]=0.003233;     pa10->me[4][7]=0.0;	     pa10->me[4][8]=0.0;        pa10->me[4][9]=0.000960;  pa10->me[4][10]=0.000033;  pa10->me[4][11]=71.923;	  pa10->me[4][12]=0.0000826;  pa10->me[4][13]=0.00926;   pa10->me[4][14]=-0.0145;
  pa10->me[5][0]=1.46;   pa10->me[5][1]=0.0;	   pa10->me[5][2]=0.003;	 pa10->me[5][3]=0.0;       pa10->me[5][4]=0.012500;     pa10->me[5][5]=0.001431;     pa10->me[5][6]=0.012500;     pa10->me[5][7]=0.0;	     pa10->me[5][8]=0.0;        pa10->me[5][9]=0.0;		  pa10->me[5][10]=0.000033;  pa10->me[5][11]=76.686;	  pa10->me[5][12]=0.0000367;  pa10->me[5][13]=0.00396;   pa10->me[5][14]=-0.0105;
  pa10->me[6][0]=0.24;   pa10->me[5][1]=0.0;	   pa10->me[5][2]=0.0;		 pa10->me[5][3]=0.0;       pa10->me[5][4]=0.001575;     pa10->me[5][5]=0.001575;     pa10->me[5][6]=0.000131;     pa10->me[5][7]=0.0;	     pa10->me[5][8]=0.0;        pa10->me[5][9]=0.0;		  pa10->me[5][10]=0.000033;  pa10->me[5][11]=76.686;	  pa10->me[5][12]=0.0000367;  pa10->me[5][13]=0.00396;   pa10->me[5][14]=-0.0105;

  return pa10;
}
*/
